﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.IO;
using Valentia.Kinection.DataTypes;
using Valentia.Kinection.Manager;
using System.Net;

namespace Valentia.Kinection.Test
{
    public partial class frmKinectExtractorTest : Form
    {
        #region DATA MEMBERS
        private static String VideoFrameDir = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "Video Frames");
        private static String DepthFrameDir = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "Depth Frames");
        private static String SkeletonFrameDir = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "Skeleton Frames");

        private bool hasSkeletonFrame, drawSkeletonOnVideo, saveSkeletonData, playingSkeletonData;
        private int sktWidth = 640, sktHeight = 480, jointAddSubtract = 6;
        private Color skeletonImageBackColor;
        private Pen skeletonBonePen;
        private Pen skeletonJointPen;
        private Pen appSkeletonLockedPen;
        private Brush backgroundBrush;
        private Graphics skeletonBmpGraphics;
        private Bitmap skeletonImage;
        private SkeletonFrameReceivedArgs lastSkeletonFrame;
        private SynchronizationContext context;
        private KinectManager kinectManager;
        #endregion

        #region CONSTRUCTOR
        static frmKinectExtractorTest()
        {
            if (!Directory.Exists(frmKinectExtractorTest.VideoFrameDir)) { Directory.CreateDirectory(frmKinectExtractorTest.VideoFrameDir); }
            if (!Directory.Exists(frmKinectExtractorTest.DepthFrameDir)) { Directory.CreateDirectory(frmKinectExtractorTest.DepthFrameDir); }
            if (!Directory.Exists(frmKinectExtractorTest.SkeletonFrameDir)) { Directory.CreateDirectory(frmKinectExtractorTest.SkeletonFrameDir); }
        }
        public frmKinectExtractorTest()
        {
            InitializeComponent();

            //ComboBox cmb = null;
            //IPAddress[] ipAddresses = Dns.GetHostEntry(Dns.GetHostName()).AddressList;
            //foreach (IPAddress address in ipAddresses)
            //{
            //    if (address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetworkV6) continue;
            //    if (address.ToString().Equals("0.0.0.0") || address.ToString().Equals("127.0.0.1")) continue;

            //    cmb.Items.Add(address.ToString());
            //}

            //cmb.Items.Insert(0, "Any"); //0.0.0.0
            //cmb.Items.Insert(1, "Local"); //127.0.0.1

            //String data = File.ReadAllText(Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "Skeleton Joints - Copy.txt"));

            this.btnMoveKinectUp.Enabled = this.btnMoveKinectDown.Enabled = false;

            this.hasSkeletonFrame = false;
            this.drawSkeletonOnVideo = this.btnDrawSkeletonOnVideo.Checked;
            this.saveSkeletonData = false;
            this.playingSkeletonData = false;

            int penWidth = 3;
            this.skeletonImageBackColor = Color.Black;
            this.skeletonBonePen = new Pen(Color.White, penWidth);
            this.skeletonJointPen = new Pen(Color.Green, penWidth);
            this.appSkeletonLockedPen = new Pen(Color.Red, penWidth);
            this.backgroundBrush = new SolidBrush(this.skeletonImageBackColor);
            this.skeletonImage = new Bitmap(this.sktWidth, this.sktHeight);
            this.skeletonBmpGraphics = Graphics.FromImage(this.skeletonImage);
            this.pbVideo.SizeMode = PictureBoxSizeMode.StretchImage;
            this.pbDepth.SizeMode = PictureBoxSizeMode.StretchImage;
            this.pbSkeleton.SizeMode = PictureBoxSizeMode.StretchImage;

            this.cboAllKinectDevices.Items.Add(String.Empty); this.cboAllKinectDevices.SelectedIndex = 0;
            this.cboLockedKinectDevices.Items.Add(String.Empty); this.cboLockedKinectDevices.SelectedIndex = 0;

            this.context = SynchronizationContext.Current;
            this.kinectManager = new KinectManager();
            this.kinectManager.KinectDeviceStatusChagned += new EventHandler<KinectDeviceStatusChagnedArgs>(kinectManager_KinectDeviceStatusChagned);
            this.kinectManager.KinectDeviceLocked += new EventHandler<KinectDeviceLockedArgs>(kinectManager_KinectDeviceLocked);
            this.kinectManager.FrameReceived += new EventHandler<FrameReceivedArgs>(kinectManager_FrameReceived);
            this.kinectManager.SkeletonFrameReceived += new EventHandler<SkeletonFrameReceivedArgs>(kinectManager_SkeletonFrameReceived);
            this.kinectManager.NotificationReceived += new EventHandler<NotificationReceivedArgs>(kinectExtractor_NotificationReceived);
            this.kinectManager.NoSkeletonFrame += new EventHandler<NoSkeletonFrameArgs>(kinectManager_NoSkeletonFrame);

            this.btnSaveSkeletonData.Checked = this.saveSkeletonData;
        }
        #endregion

        #region THREADS AND CONTEXT SYNCHRONIZATION METHODS
        private void DrawRecoredSkeletons(object state)
        {
            try
            {
                if (this.playingSkeletonData) return;

                this.playingSkeletonData = true;

                int delay = 1000 / 30;
                foreach (String file in Directory.GetFiles(frmKinectExtractorTest.SkeletonFrameDir, "*.dat"))
                {
                    SkeletonFrameReceivedArgs ee = SkeletonFrameReceivedArgs.FromBytes(File.ReadAllBytes(file));
                    this.context.Send(this.ShowSkeleton, ee);
                    Thread.Sleep(delay);
                }
            }
            finally { this.playingSkeletonData = false; }
        }

        private void KinectDeviceStatunChanged(object state)
        {
            KinectDeviceStatusChagnedArgs e = state as KinectDeviceStatusChagnedArgs;
            if (e == null) return;

            this.AddKinectToAllKinectCombo(e.KinectDevice);
        }

        private void KinectDeviceLocked(object state)
        {
            KinectDeviceLockedArgs e = state as KinectDeviceLockedArgs;
            if (e == null) return;

            this.AddKinectToAllKinectCombo(e.KinectDevice);
            this.AddKinectToLockedCombo(e.KinectDevice, e.Locked);
        }

        private void ShowVideoImage(object state)
        {
            FrameReceivedArgs frameReceivedArgs = state as FrameReceivedArgs;
            if (frameReceivedArgs == null || !this.IsKinectSelected(frameReceivedArgs.KinectDevice)) { this.pbVideo.Image = null; return; }

            if (this.drawSkeletonOnVideo)
            {
                try
                {
                    if (this.hasSkeletonFrame && this.lastSkeletonFrame != null)
                    {
                        Graphics videoBmpGraphics = Graphics.FromImage(frameReceivedArgs.Bitmap);
                        this.DrawSkeletons(videoBmpGraphics, this.lastSkeletonFrame);
                    }
                }
                catch { }
            }

            this.pbVideo.Image = frameReceivedArgs.Bitmap;
            //frameReceivedArgs.Bitmap.Save(Path.Combine(frmKinectExtractorTest.VideoFrameDir, this.FrameFileName(true)));
        }

        private void ShowDepthImage(object state)
        {
            FrameReceivedArgs frameReceivedArgs = state as FrameReceivedArgs;
            if (frameReceivedArgs == null || !this.IsKinectSelected(frameReceivedArgs.KinectDevice)) { this.pbDepth.Image = null; return; }

            this.pbDepth.Image = frameReceivedArgs.Bitmap;
            //frameReceivedArgs.Bitmap.Save(Path.Combine(frmKinectExtractorTest.DepthFrameDir, this.FrameFileName(true)));
        }

        private void ShowSkeleton(object state)
        {
            SkeletonFrameReceivedArgs sktFrame = state as SkeletonFrameReceivedArgs;
            if (sktFrame == null || (!this.playingSkeletonData && !this.IsKinectSelected(sktFrame.KinectDevice)))
            {
                this.pbSkeleton.Image = null;
                return;
            }

            //int width = 640, height = 480;
            //this.graphics.FillRectangle(this.whiteBrush, 0, 0, width, height);
            this.skeletonBmpGraphics.Clear(this.skeletonImageBackColor);

            this.DrawSkeletons(this.skeletonBmpGraphics, sktFrame);

            this.pbSkeleton.Image = this.skeletonImage;
            this.lastSkeletonFrame = sktFrame;

            if (this.saveSkeletonData && !this.playingSkeletonData)
            {
                File.WriteAllBytes(Path.Combine(frmKinectExtractorTest.SkeletonFrameDir, this.FrameFileName(false)), sktFrame.GetBytes());
            }
        }

        private void ShowNoSkeleton(object state)
        {
            this.skeletonBmpGraphics.Clear(this.skeletonImageBackColor);
            this.pbSkeleton.Image = this.skeletonImage;
        }

        private void ShowNotification(object state)
        {
            NotificationReceivedArgs notificationArgs = state as NotificationReceivedArgs;
            if (notificationArgs == null) return;

            String expString = String.Empty;
            String message = notificationArgs.Message;
            if (notificationArgs.Exception != null)
            {
                message = String.Format("{0}. {1}", message, notificationArgs.Exception.Message);
                expString = notificationArgs.Exception.StackTrace + Environment.NewLine;
            }

            File.AppendAllText(
                Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "NotificationText.txt"),
                String.Format(
                    "{0} --> {1}: {2}{3}{4}", DateTime.Now.ToString("HH:mm:ss.fff"), notificationArgs.NotificationType.ToString(), message,
                    Environment.NewLine, expString));

            ListViewItem item =
                new ListViewItem(
                    new String[] { String.Empty, DateTime.Now.ToString("HH:mm:ss.fff"), notificationArgs.NotificationType.ToString(), message });
            this.listView1.Items.Add(item);
            item.Selected = true;
        }
        #endregion

        #region FORMS AND CONTROLS EVENT HANDLER METHODS
        private void frmKinectExtractorTest_Load(object sender, EventArgs e)
        {

        }

        private void frmKinectExtractorTest_FormClosing(object sender, FormClosingEventArgs e)
        {
            if (!this.kinectManager.Started) return;

            this.kinectManager.Stop();
        }

        private void btnStartStopExtractor_Click(object sender, EventArgs e)
        {
            try
            {
                switch (this.kinectManager.Started)
                {
                    case true: this.kinectManager.Stop(); break;
                    default: this.kinectManager.Start(); break;
                }
            }
            catch (Exception exp)
            {
                MessageBox.Show(this, exp.Message, Application.ProductName, MessageBoxButtons.OK, MessageBoxIcon.Error);
            }
            finally
            {
                this.btnStartStopExtractor.Text = this.kinectManager.Started ? "Stop Extractor" : "Startp Extractor";
                this.btnMoveKinectUp.Enabled = this.btnMoveKinectDown.Enabled = this.kinectManager.Started;
            }
        }

        private void btnMoveKinectUp_Click(object sender, EventArgs e)
        {
            if (this.cboAllKinectDevices.SelectedIndex <= 0) { this.kinectManager.MoveKinectUp(); }
            else { this.kinectManager.MoveKinectUp(this.cboAllKinectDevices.Text); }
        }

        private void btnMoveKinectDown_Click(object sender, EventArgs e)
        {
            if (this.cboAllKinectDevices.SelectedIndex <= 0) { this.kinectManager.MoveKinectDown(); }
            else { this.kinectManager.MoveKinectDown(this.cboAllKinectDevices.Text); }
        }

        private void btnDrawSkeletonOnVideo_Click(object sender, EventArgs e)
        {
            this.drawSkeletonOnVideo = this.btnDrawSkeletonOnVideo.Checked;
        }

        private void btnSaveSkeletonData_Click(object sender, EventArgs e)
        {
            this.saveSkeletonData = this.btnSaveSkeletonData.Checked;
        }

        private void btnPlaySkeletonData_Click(object sender, EventArgs e)
        {
            ThreadPool.QueueUserWorkItem(this.DrawRecoredSkeletons, null);
        }

        private void btnClearNotifications_Click(object sender, EventArgs e) { this.listView1.Items.Clear(); }
        #endregion

        #region KINECT EXTRACTOR EVENT HANDLER METHDOS
        private void kinectManager_KinectDeviceStatusChagned(object sender, KinectDeviceStatusChagnedArgs e)
        {
            this.context.Send(this.KinectDeviceStatunChanged, e);
        }

        private void kinectManager_KinectDeviceLocked(object sender, KinectDeviceLockedArgs e)
        {
            this.context.Send(this.KinectDeviceLocked, e);
        }

        private void kinectManager_FrameReceived(object sender, FrameReceivedArgs e)
        {
            switch (e.FrameType)
            {
                case FrameReceivedArgs.FrameReceivedType.Video: this.context.Send(this.ShowVideoImage, e); break;
                case FrameReceivedArgs.FrameReceivedType.Depth: this.context.Send(this.ShowDepthImage, e); break;
            }
        }

        private void kinectManager_SkeletonFrameReceived(object sender, SkeletonFrameReceivedArgs e)
        {
            this.hasSkeletonFrame = true;
            this.context.Send(this.ShowSkeleton, e);
        }

        private void kinectExtractor_NotificationReceived(object sender, NotificationReceivedArgs e)
        {
            this.context.Send(this.ShowNotification, e);
        }

        private void kinectManager_NoSkeletonFrame(object sender, NoSkeletonFrameArgs e)
        {
            this.hasSkeletonFrame = false;
            this.context.Send(this.ShowNoSkeleton, null);
        }
        #endregion

        #region PRIVATE METHODS
        private String FrameFileName(bool videoOrDepth) { return (DateTime.Now.ToString("HHmmssfff") + (videoOrDepth ? ".bmp" : ".dat")); }

        private bool IsKinectSelected(String kinectDevice)
        {
            return (String.Compare(this.cboLockedKinectDevices.Text, kinectDevice, 0) == 0);
        }

        private void AddKinectToAllKinectCombo(String kinectDevice)
        {
            if (this.cboAllKinectDevices.Items.Contains(kinectDevice)) return;

            this.cboAllKinectDevices.Items.Add(kinectDevice);
        }

        private void AddKinectToLockedCombo(String kinectDevice, bool locked)
        {
            switch (locked)
            {
                case true: if (!this.cboLockedKinectDevices.Items.Contains(kinectDevice)) { this.cboLockedKinectDevices.Items.Add(kinectDevice); } break;
                default: if (this.cboLockedKinectDevices.Items.Contains(kinectDevice)) { this.cboLockedKinectDevices.Items.Remove(kinectDevice); } break;
            }
        }

        private void DrawSkeletons(Graphics bmpGraphics, SkeletonFrameReceivedArgs sktFrame)
        {
            bool writeWSProtocolData = false;

            String lf = "\n", dl = "$", forTeting = String.Empty;
            if (writeWSProtocolData) { forTeting = 3 + lf + "Test Device" + lf + sktFrame.Skeletons.Length.ToString() + lf; }

            foreach (SkeletonJointCollection joints in sktFrame.Skeletons)
            {
                if (writeWSProtocolData)
                {
                    forTeting += joints.TrackingID + dl + joints.UserIndex + dl + "1" + dl + joints.Count + dl;

                    foreach (SkeletonJointInfo sj in joints)
                    {
                        forTeting += ((int)sj.JointID) + "," + sj.Vector.W + "," + sj.Vector.X + "," + sj.Vector.Y + "," + sj.Vector.Z + dl;
                    }

                    forTeting = forTeting.Trim((lf + dl).ToArray()).Trim() + lf;
                }

                this.DrawBones(bmpGraphics, joints);
            }

            if (writeWSProtocolData)
            {
                forTeting = forTeting.Trim((lf + dl).ToArray()).Trim() + lf;
                File.AppendAllText(Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "Skeleton Joints.txt"), forTeting);
            }
        }

        private void DrawBones(Graphics bmpGraphics, SkeletonJointCollection joints)
        {
            this.DrawBonesLine(bmpGraphics, joints, SkeletonJointID.HipCenter, SkeletonJointID.Spine, SkeletonJointID.ShoulderCenter, SkeletonJointID.Head);
            this.DrawBonesLine(bmpGraphics, joints, SkeletonJointID.ShoulderCenter, SkeletonJointID.ShoulderLeft, SkeletonJointID.ElbowLeft, SkeletonJointID.WristLeft, SkeletonJointID.HandLeft);
            this.DrawBonesLine(bmpGraphics, joints, SkeletonJointID.ShoulderCenter, SkeletonJointID.ShoulderRight, SkeletonJointID.ElbowRight, SkeletonJointID.WristRight, SkeletonJointID.HandRight);
            this.DrawBonesLine(bmpGraphics, joints, SkeletonJointID.HipCenter, SkeletonJointID.HipLeft, SkeletonJointID.KneeLeft, SkeletonJointID.AnkleLeft, SkeletonJointID.FootLeft);
            this.DrawBonesLine(bmpGraphics, joints, SkeletonJointID.HipCenter, SkeletonJointID.HipRight, SkeletonJointID.KneeRight, SkeletonJointID.AnkleRight, SkeletonJointID.FootRight);

            foreach (SkeletonJointInfo joint in joints)
            {
                bmpGraphics.DrawLine(this.skeletonJointPen, (float)(joint.ColorX - this.jointAddSubtract), (float)(joint.ColorY), (float)(joint.ColorX + this.jointAddSubtract), (float)(joint.ColorY));
            }

            //if (!this.appLockedSkeletonID.HasValue || this.appLockedSkeletonID.Value != joints.TrackingID) return;

            //SkeletonJoint head = joints[SkeletonJointID.Head];
            //bmpGraphics.DrawEllipse(this.appSkeletonLockedPen, new Rectangle((int)(head.ColorX) - 10, (int)(head.ColorY) - 10, 20, 20));

            //int elboShoulderYDiff = 10, handHeadYDiff = 10;
            //SkeletonJoint elbowRight = joints[SkeletonJointID.ElbowRight];
            //SkeletonJoint shoulderRight = joints[SkeletonJointID.ShoulderRight];
            //if (Math.Abs(shoulderRight.ColorY - elbowRight.ColorY) <= elboShoulderYDiff)
            //{
            //    SkeletonJoint handRight = joints[SkeletonJointID.HandRight];
            //    if (Math.Abs(handRight.ColorY - head.ColorY) <= handHeadYDiff)
            //    {
            //        if (!this.shoulderElbowAlighTime.HasValue)
            //        {
            //            this.shoulderElbowAlighTime = DateTime.Now;
            //            this.firstHandXPoint = handRight.ColorX;
            //            this.movingRight = null;
            //            return;
            //        }

            //        if (!this.movingRight.HasValue)
            //        {
            //            this.firstHandXPoint
            //        }
            //    }
            //}
            //else { this.shoulderElbowAlighTime = null; }
        }

        private bool? movingRight = null;
        private double? firstHandXPoint = null;

        private void DrawBonesLine(Graphics bmpGraphics, SkeletonJointCollection joints, params SkeletonJointID[] jointIDs)
        {
            List<Point> points = new List<Point>(jointIDs.Length);
            foreach (SkeletonJointID jointID in jointIDs) { points.Add(joints[jointID].ToPoint()); }
            bmpGraphics.DrawLines(this.skeletonBonePen, points.ToArray());
        }
        #endregion
    }
}